#include "vl53l0x.h"
#include <stdio.h>
#include "main.h"
#include "usart.h"
#include <stdbool.h>
// #include "./SysTick/bsp_SysTick.h"

VL53L0X_Dev_t multi_vl53l0x_dev[6];
_VL53L0X_adjust Vl53l0x_data;
flow_control_paras_ flow_control_paras;
char VL53L0XMsg[128];

VL53L0X_DeviceInfo_t vl53l0x_dev_info; /*设备ID版本信息*/
uint8_t AjustOK = 0;                   /*校准标志位*/

/*0：默认;1:高精度;2:长距离;3:高速*/
mode_data Mode_data[] =
{
  {(FixPoint1616_t)(0.25 * 65536),
    (FixPoint1616_t)(18 * 65536),
    33000,
    14,
    10}, /*默认*/

  {(FixPoint1616_t)(0.25 * 65536),
    (FixPoint1616_t)(18 * 65536),
    200000,
    14,
    10}, /*高精度*/

  {(FixPoint1616_t)(0.1 * 65536),
    (FixPoint1616_t)(60 * 65536),
    33000,
    18,
		
    14}, /*长距离*/

  {(FixPoint1616_t)(0.25 * 65536),
    (FixPoint1616_t)(32 * 65536),
    20000,
    14,
    10}, /*高速*/
};

/**
 * @brief  打印错误信息
 * @param  Status详情看VL53L0X_Error参数的定义
 * @retval 无
 */
void print_pal_error(VL53L0X_Error Status)
{
  char buf[VL53L0X_MAX_STRING_LENGTH];

  /*根据Status状态获取错误信息字符串*/
  VL53L0X_GetPalErrorString(Status, buf);

  /*打印状态和错误信息*/
  sprintf(VL53L0XMsg, "API Status: %i : %s\r\n", Status, buf);
  HAL_UART_Transmit(&huart4, (uint8_t *)VL53L0XMsg, strlen(VL53L0XMsg), 0xF);
}

/**
 * @brief  配置VL53L0X设备I2C地址
 * @param  dev:设备I2C参数结构体
 * @param  Status：详情看VL53L0X_Error参数的定义
 * @param  newaddr:设备新I2C地址
 * @retval 错误信息
 */
VL53L0X_Error vl53l0x_Addr_set(VL53L0X_Dev_t *dev, uint8_t newaddr)
{
  uint16_t Id;
  uint8_t FinalAddress;
  VL53L0X_Error Status = VL53L0X_ERROR_NONE;
  uint8_t sta = 0x00;

  FinalAddress = newaddr;

  /*新设备I2C地址与旧地址一致,直接退出*/
  if (FinalAddress == dev->I2cDevAddr)
    return VL53L0X_ERROR_NONE;

  /*在进行第一个寄存器访问之前设置I2C标准模式(400Khz)*/
//  Status = VL53L0X_WrByte(dev, 0x88, 0x00);
//  if (Status != VL53L0X_ERROR_NONE)
//  {
//    /*设置I2C标准模式出错*/
//    sta = 0x01;
//    goto set_error;
//  }
  /*尝试使用默认的0x52地址读取一个寄存器*/
  Status = VL53L0X_RdWord(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
  if (Status != VL53L0X_ERROR_NONE)
  {
    /*读取寄存器出错*/
    sta = 0x02;
    goto set_error;
  }
  if (Id == 0xEEAA)
  {
    /*设置设备新的I2C地址*/
    Status = VL53L0X_SetDeviceAddress(dev, FinalAddress);
    if (Status != VL53L0X_ERROR_NONE)
    {
      /*设置I2C地址出错*/
      sta = 0x03;
      goto set_error;
    }
    /*修改参数结构体的I2C地址*/
    dev->I2cDevAddr = FinalAddress;
    /*检查新的I2C地址读写是否正常*/
    Status = VL53L0X_RdWord(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
    if (Status != VL53L0X_ERROR_NONE)
    {
      /*新I2C地址读写出错*/
      sta = 0x04;
      goto set_error;
    }
  }
set_error:
  if (Status != VL53L0X_ERROR_NONE)
  {
    /*打印错误信息*/
    print_pal_error(Status);
  }
  if (sta != 0)
    sprintf(VL53L0XMsg, "sta:0x%x\r\n", sta);
  return Status;
}

/**
 * @brief  vl53l0x复位函数
 * @param  dev:设备I2C参数结构体
 * @retval 无
 */
VL53L0X_Error vl53l0x_reset(VL53L0X_Dev_t *dev)
{
  uint8_t addr;
  VL53L0X_Error Status = VL53L0X_ERROR_NONE;

  /*保存设备原I2C地址*/
  addr = dev->I2cDevAddr;
  /*失能VL53L0X*/
  LV_DISABLE(dev->gpio_port, dev->gpio_pin);
  hal_delay_ms(30);

  /*使能VL53L0X,让传感器处于工作(I2C地址会恢复默认0X52)*/
  LV_ENABLE(dev->gpio_port, dev->gpio_pin);
  hal_delay_ms(30);

  dev->I2cDevAddr = 0x52;

  /*设置VL53L0X传感器原来上电前原I2C地址*/
  Status = vl53l0x_Addr_set(dev, addr);
  if(VL53L0X_ERROR_NONE != Status)
  {
    goto vl53l0x_reset_error;
  }
  
  Status = VL53L0X_DataInit(dev);
  if(VL53L0X_ERROR_NONE != Status)
  {
    goto vl53l0x_reset_error;
  }
  
vl53l0x_reset_error:
  return Status;
}

/**
 * @brief  初始化vl53l0x
 * @param  dev:设备I2C参数结构体
 * @retval 状态信息
 */
VL53L0X_Error vl53l0x_init(VL53L0X_Dev_t *dev, uint8_t new_addr)
{
  VL53L0X_Error Status = VL53L0X_ERROR_NONE;
  VL53L0X_Dev_t *pMyDevice = dev;

  pMyDevice->I2cDevAddr = VL53L0X_Addr; /*I2C地址(上电默认0x52)*/
  pMyDevice->comms_type = 1;            /*I2C通信模式*/
  pMyDevice->comms_speed_khz = 400;     /*I2C通信速率*/

  LV_DISABLE(pMyDevice->gpio_port, pMyDevice->gpio_pin); /*失能VL53L0X*/
  hal_delay_ms(30);
  LV_ENABLE(pMyDevice->gpio_port, pMyDevice->gpio_pin); /*使能VL53L0X,让传感器处于工作*/
  hal_delay_ms(30);

  Status = vl53l0x_Addr_set(pMyDevice, new_addr); /*设置VL53L0X传感器I2C地址*/

  if (Status != VL53L0X_ERROR_NONE)
    goto error;
  Status = VL53L0X_DataInit(pMyDevice); /*设备初始化*/
  if (Status != VL53L0X_ERROR_NONE)
    goto error;
  hal_delay_ms(2);

  Status = VL53L0X_GetDeviceInfo(pMyDevice, &vl53l0x_dev_info); /*获取设备ID信息*/
  if (Status != VL53L0X_ERROR_NONE)
    goto error;

  if (Vl53l0x_data.adjustok == 0xAA) /*已校准*/
    AjustOK = 1;
  else /*没校准*/
    AjustOK = 0;

error:
  if (Status != VL53L0X_ERROR_NONE)
  {
    print_pal_error(Status); /*打印错误信息*/
    return Status;
  }

  return Status;
}

void bsp_init()
{
  flow_control_paras.tranfer_complete = 0;

  multi_vl53l0x_dev[0].gpio_port = GPIOA;
  multi_vl53l0x_dev[0].gpio_pin = GPIO_PIN_3;

  multi_vl53l0x_dev[1].gpio_port = GPIOA;
  multi_vl53l0x_dev[1].gpio_pin = GPIO_PIN_4;

  multi_vl53l0x_dev[2].gpio_port = GPIOA;
  multi_vl53l0x_dev[2].gpio_pin = GPIO_PIN_5;

  multi_vl53l0x_dev[3].gpio_port = GPIOA;
  multi_vl53l0x_dev[3].gpio_pin = GPIO_PIN_6;

  multi_vl53l0x_dev[4].gpio_port = GPIOA;
  multi_vl53l0x_dev[4].gpio_pin = GPIO_PIN_7;

  multi_vl53l0x_dev[5].gpio_port = GPIOC;
  multi_vl53l0x_dev[5].gpio_pin = GPIO_PIN_4;

  for (int index = 0; index < 6; index++)
  {
    #if VL53L0X_DEBUG
    sprintf(VL53L0XMsg, "Init: %d\r\n", index);
    HAL_UART_Transmit(&huart4, (uint8_t *)VL53L0XMsg, strlen(VL53L0XMsg), 0xF);
    #endif

    while (vl53l0x_init(&multi_vl53l0x_dev[index], 0x54 + 2 * index))  //0x54 + 2 * index
    {
      // 初始化失败;
      osDelay(1);
      sprintf(VL53L0XMsg, "Init Failed: %d\r\n", index);
      HAL_UART_Transmit(&huart4, (uint8_t *)VL53L0XMsg, strlen(VL53L0XMsg), 0xF);
    }
    
    #if VL53L0X_DEBUG
    sprintf(VL53L0XMsg, "Set Mode: %d, AjustOK?: %d\r\n", index, AjustOK);
    HAL_UART_Transmit(&huart4, (uint8_t *)VL53L0XMsg, strlen(VL53L0XMsg), 0xF);
    #endif

    int i = 0;
    /*配置测量模式*/
    while (vl53l0x_set_mode(&multi_vl53l0x_dev[index], 3))
    {
      // 模式设置失败;
      hal_delay_ms(50);
      i++;
      if (i == 20)
      {
        return;
      }
    }
  }
  return;
}

static void measure_once(VL53L0X_Dev_t *dev, uint8_t new_addr)
{
  vl53l0x_general_test(dev);
}

bool DBG_Err(void)
{
	return false;
}



	uint16_t TmpData  = 0;
	uint16_t Addr = 0;
bool DBG_PowerOn_SetAdd(void)
{
	bool Ret =false;
	VL53L0X_Error Status = VL53L0X_ERROR_NONE;

	uint8_t sta;
	uint8_t idx = 1;
	uint8_t i =0;
	bool Exit52 = true;
	
	flow_control_paras.tranfer_complete = 0;

  multi_vl53l0x_dev[0].gpio_port = SHT1_GPIO_Port;
  multi_vl53l0x_dev[0].gpio_pin = SHT1_Pin;
	multi_vl53l0x_dev[0].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[1].gpio_port = SHT2_GPIO_Port;
  multi_vl53l0x_dev[1].gpio_pin = SHT2_Pin;
	multi_vl53l0x_dev[1].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[2].gpio_port = SHT3_GPIO_Port;
  multi_vl53l0x_dev[2].gpio_pin = SHT3_Pin;
	multi_vl53l0x_dev[2].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[3].gpio_port = SHT4_GPIO_Port;
  multi_vl53l0x_dev[3].gpio_pin = SHT4_Pin;
	multi_vl53l0x_dev[3].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[4].gpio_port = SHT5_GPIO_Port;
  multi_vl53l0x_dev[4].gpio_pin = SHT5_Pin;
	multi_vl53l0x_dev[4].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[5].gpio_port = SHT6_GPIO_Port;
  multi_vl53l0x_dev[5].gpio_pin = SHT6_Pin;
	multi_vl53l0x_dev[5].I2cDevAddr = 0x52;
	
	/*休眠所有VL53L0X*/
	for(i = 0; i< 6; i++)
	{
		LV_DISABLE(multi_vl53l0x_dev[i].gpio_port, multi_vl53l0x_dev[i].gpio_pin); 
		osDelay(30);
	}
	
//	for(i = 0; i< 6; i++)
//	{
//		LV_ENABLE(multi_vl53l0x_dev[i].gpio_port, multi_vl53l0x_dev[i].gpio_pin); 
//		osDelay(30);
//	}
	
	osDelay(30);
	
	/*逐个设置 VL53L0X 地址*/
	for(i = 0; i< 6; i++)
	{
		
		LV_ENABLE(multi_vl53l0x_dev[i].gpio_port, multi_vl53l0x_dev[i].gpio_pin); 
		
		osDelay(30);
		
		// --1-- 读取参考寄存器值
		TmpData = 0;
		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[i], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
		if(Status != VL53L0X_ERROR_NONE)
		{	return DBG_Err(); }
		
		if(TmpData != 0xEEAA)
		{	return DBG_Err(); }
		
		// --2-- 设置设备新的I2C地址
		Addr = 0;
		Status = VL53L0X_SetDeviceAddress(&multi_vl53l0x_dev[i], (0x60 + i*2));
		multi_vl53l0x_dev[i].I2cDevAddr = 0x60 + i*2;
		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[i], VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,&Addr);
		Addr = (Addr & 0xFF00) >> 8;
		if(Status != VL53L0X_ERROR_NONE)
		{	return DBG_Err(); }
		
		if((2*Addr) != (0x60 + i*2))
		{	return DBG_Err(); }
		
		// --3-- 读取设置完新地址的参考寄存器的值
		TmpData = 0;
		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[i], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
		if(Status != VL53L0X_ERROR_NONE)
		{	return DBG_Err(); }
		
		if(TmpData != 0xEEAA)
		{	return DBG_Err(); }
	
		/* --4-- 重新尝试读取0x52的 I2c设备,防止设置失败 */
		osDelay(100);  //延时不能去除，延时一段时间后52就无法访问了
		TmpData = 0;
		multi_vl53l0x_dev[i].I2cDevAddr = 0x52;
		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[i], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
		multi_vl53l0x_dev[i].I2cDevAddr = 0x60 + i*2;
		if(Status != VL53L0X_ERROR_NONE)    //说明通信上了，则报错，地址未修改成功
		{	return DBG_Err(); }
		
		if(TmpData == 0xEEAA)  //说明通信上了，则报错，地址未修改成功
		{	return DBG_Err(); }
		
	}
	
	
	return false;
}


void DBG_VL_Init(void)
{
	VL53L0X_Error Status = VL53L0X_ERROR_NONE;
	uint16_t TmpData;
	uint16_t Addr;
	uint8_t sta;
	uint8_t idx = 0;
	
	flow_control_paras.tranfer_complete = 0;

  multi_vl53l0x_dev[0].gpio_port = GPIOA;
  multi_vl53l0x_dev[0].gpio_pin = GPIO_PIN_3;
	multi_vl53l0x_dev[0].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[1].gpio_port = GPIOA;
  multi_vl53l0x_dev[1].gpio_pin = GPIO_PIN_4;
	multi_vl53l0x_dev[1].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[2].gpio_port = GPIOA;
  multi_vl53l0x_dev[2].gpio_pin = GPIO_PIN_5;
	multi_vl53l0x_dev[2].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[3].gpio_port = GPIOA;
  multi_vl53l0x_dev[3].gpio_pin = GPIO_PIN_6;
	multi_vl53l0x_dev[3].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[4].gpio_port = GPIOA;
  multi_vl53l0x_dev[4].gpio_pin = GPIO_PIN_7;
	multi_vl53l0x_dev[4].I2cDevAddr = 0x52;

  multi_vl53l0x_dev[5].gpio_port = GPIOC;
  multi_vl53l0x_dev[5].gpio_pin = GPIO_PIN_4;
	multi_vl53l0x_dev[5].I2cDevAddr = 0x52;
	
	LV_DISABLE(multi_vl53l0x_dev[0].gpio_port, multi_vl53l0x_dev[0].gpio_pin); /*失能VL53L0X*/
  osDelay(30);
	LV_ENABLE(multi_vl53l0x_dev[0].gpio_port, multi_vl53l0x_dev[0].gpio_pin); /*失能VL53L0X*/
	osDelay(30);
	
	LV_DISABLE(multi_vl53l0x_dev[1].gpio_port, multi_vl53l0x_dev[1].gpio_pin); /*失能VL53L0X*/
  osDelay(30);
//	LV_ENABLE(multi_vl53l0x_dev[1].gpio_port, multi_vl53l0x_dev[1].gpio_pin); /*失能VL53L0X*/
	osDelay(30);
	
	LV_DISABLE(multi_vl53l0x_dev[2].gpio_port, multi_vl53l0x_dev[2].gpio_pin); /*失能VL53L0X*/
  osDelay(30);
//	LV_ENABLE(multi_vl53l0x_dev[2].gpio_port, multi_vl53l0x_dev[2].gpio_pin); /*失能VL53L0X*/
	osDelay(30);
	
	LV_DISABLE(multi_vl53l0x_dev[3].gpio_port, multi_vl53l0x_dev[3].gpio_pin); /*失能VL53L0X*/
  osDelay(30);
//	LV_ENABLE(multi_vl53l0x_dev[3].gpio_port, multi_vl53l0x_dev[3].gpio_pin); /*失能VL53L0X*/
	osDelay(30);
	
	LV_DISABLE(multi_vl53l0x_dev[4].gpio_port, multi_vl53l0x_dev[4].gpio_pin); /*失能VL53L0X*/
  osDelay(30);
//	LV_ENABLE(multi_vl53l0x_dev[4].gpio_port, multi_vl53l0x_dev[4].gpio_pin); /*失能VL53L0X*/
	osDelay(30);
	
	LV_DISABLE(multi_vl53l0x_dev[5].gpio_port, multi_vl53l0x_dev[5].gpio_pin); /*失能VL53L0X*/
  osDelay(30);
//	LV_ENABLE(multi_vl53l0x_dev[5].gpio_port, multi_vl53l0x_dev[5].gpio_pin); /*失能VL53L0X*/
	osDelay(30);
	
	osDelay(30);
	
//	LV_DISABLE(multi_vl53l0x_dev[0].gpio_port, multi_vl53l0x_dev[0].gpio_pin); /*失能VL53L0X*/
//  osDelay(30);
//	LV_DISABLE(multi_vl53l0x_dev[1].gpio_port, multi_vl53l0x_dev[1].gpio_pin); /*失能VL53L0X*/
//  osDelay(30);
//	LV_DISABLE(multi_vl53l0x_dev[2].gpio_port, multi_vl53l0x_dev[2].gpio_pin); /*失能VL53L0X*/
//  osDelay(30);
//	LV_DISABLE(multi_vl53l0x_dev[3].gpio_port, multi_vl53l0x_dev[3].gpio_pin); /*失能VL53L0X*/
//  osDelay(30);
//	LV_DISABLE(multi_vl53l0x_dev[4].gpio_port, multi_vl53l0x_dev[4].gpio_pin); /*失能VL53L0X*/
//  osDelay(30);
//	LV_DISABLE(multi_vl53l0x_dev[5].gpio_port, multi_vl53l0x_dev[5].gpio_pin); /*失能VL53L0X*/
//  osDelay(30);
	
	
	LV_ENABLE(multi_vl53l0x_dev[0].gpio_port, multi_vl53l0x_dev[0].gpio_pin); /*失能VL53L0X*/
  osDelay(30);
	TmpData = 0;
//	for(idx = 0; idx <6; idx++)
//	{
		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[0], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
		if(Status != VL53L0X_ERROR_NONE)
		{
			/*读取寄存器出错*/
			sta = 0x02;
			return;
		}
		if(TmpData == 0xEEAA)
		{
			/*设置设备新的I2C地址*/
			Status = VL53L0X_SetDeviceAddress(&multi_vl53l0x_dev[0], 0x54);
			multi_vl53l0x_dev[0].I2cDevAddr = 0x54;
			VL53L0X_RdWord(&multi_vl53l0x_dev[0], VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,&Addr);
		}
		
		
//		LV_ENABLE(multi_vl53l0x_dev[1].gpio_port, multi_vl53l0x_dev[1].gpio_pin); /*失能VL53L0X*/
		osDelay(30);
		TmpData = 0;
		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[1], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
		if(Status != VL53L0X_ERROR_NONE)
		{
			/*读取寄存器出错*/
			sta = 0x02;
			return;
		}
		if(TmpData == 0xEEAA)
		{
			/*设置设备新的I2C地址*/
			Status = VL53L0X_SetDeviceAddress(&multi_vl53l0x_dev[1], 0x56);
			multi_vl53l0x_dev[1].I2cDevAddr = 0x56;
			VL53L0X_RdWord(&multi_vl53l0x_dev[1], VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,&Addr);
		}
//		
//		LV_ENABLE(multi_vl53l0x_dev[2].gpio_port, multi_vl53l0x_dev[2].gpio_pin); /*失能VL53L0X*/
//		osDelay(30);
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[2], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			Status = VL53L0X_SetDeviceAddress(&multi_vl53l0x_dev[2], 0x58);
//			multi_vl53l0x_dev[2].I2cDevAddr = 0x58;
//			VL53L0X_RdWord(&multi_vl53l0x_dev[2], VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,&Addr);
//		}
//		
//		LV_ENABLE(multi_vl53l0x_dev[3].gpio_port, multi_vl53l0x_dev[3].gpio_pin); /*失能VL53L0X*/
//		osDelay(30);
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[3], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			Status = VL53L0X_SetDeviceAddress(&multi_vl53l0x_dev[3], 0x5A);
//			multi_vl53l0x_dev[3].I2cDevAddr = 0x5A;
//			VL53L0X_RdWord(&multi_vl53l0x_dev[3], VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,&Addr);
//		}
//		
//		LV_ENABLE(multi_vl53l0x_dev[4].gpio_port, multi_vl53l0x_dev[4].gpio_pin); /*失能VL53L0X*/
//		osDelay(30);
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[4], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			Status = VL53L0X_SetDeviceAddress(&multi_vl53l0x_dev[4], 0x5C);
//			multi_vl53l0x_dev[4].I2cDevAddr = 0x5C;
//			VL53L0X_RdWord(&multi_vl53l0x_dev[4], VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,&Addr);
//		}
//		
//		LV_ENABLE(multi_vl53l0x_dev[5].gpio_port, multi_vl53l0x_dev[5].gpio_pin); /*失能VL53L0X*/
//		osDelay(30);
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[5], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			Status = VL53L0X_SetDeviceAddress(&multi_vl53l0x_dev[5], 0x5E);
//			multi_vl53l0x_dev[5].I2cDevAddr = 0x5E;
//			VL53L0X_RdWord(&multi_vl53l0x_dev[5], VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,&Addr);
//		}
//	}
	
		
//	LV_DISABLE(multi_vl53l0x_dev[0].gpio_port, multi_vl53l0x_dev[0].gpio_pin); /*失能VL53L0X*/
//  hal_delay_ms(30);
//	
//	LV_ENABLE(multi_vl53l0x_dev[1].gpio_port, multi_vl53l0x_dev[1].gpio_pin); /*失能VL53L0X*/
//  hal_delay_ms(30);
//	
//	LV_ENABLE(multi_vl53l0x_dev[2].gpio_port, multi_vl53l0x_dev[2].gpio_pin); /*失能VL53L0X*/
//  hal_delay_ms(30);
//	
//	LV_DISABLE(multi_vl53l0x_dev[3].gpio_port, multi_vl53l0x_dev[3].gpio_pin); /*失能VL53L0X*/
//  hal_delay_ms(30);
//	
//	LV_DISABLE(multi_vl53l0x_dev[4].gpio_port, multi_vl53l0x_dev[4].gpio_pin); /*失能VL53L0X*/
//  hal_delay_ms(30);
//	
//	LV_DISABLE(multi_vl53l0x_dev[5].gpio_port, multi_vl53l0x_dev[5].gpio_pin); /*失能VL53L0X*/
//  hal_delay_ms(30);
		
//		multi_vl53l0x_dev[0].I2cDevAddr = 0x52;
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[0], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			sta = 0x03;
//		}
//		
//		multi_vl53l0x_dev[1].I2cDevAddr = 0x52;
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[1], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			sta = 0x03;
//		}
//		
//		multi_vl53l0x_dev[2].I2cDevAddr = 0x60;
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[2], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			sta = 0x03;
//		}
//		
//		multi_vl53l0x_dev[3].I2cDevAddr = 0x62;
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[3], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			sta = 0x03;
//		}
//		
//		multi_vl53l0x_dev[4].I2cDevAddr = 0x64;
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[4], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			sta = 0x03;
//		}
//		
//		
//		multi_vl53l0x_dev[5].I2cDevAddr = 0x66;
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[5], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			/*设置设备新的I2C地址*/
//			sta = 0x03;
//		}
//		
//		sta = 0x03;
	
//	for(idx = 0; idx <6; idx++)
//	{
//		multi_vl53l0x_dev[idx].I2cDevAddr = 0x52;
//		TmpData = 0;
//		Status = VL53L0X_RdWord(&multi_vl53l0x_dev[idx], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//		if(Status != VL53L0X_ERROR_NONE)
//		{
//			/*读取寄存器出错*/
//			sta = 0x02;
//			return;
//		}
//		if(TmpData == 0xEEAA)
//		{
//			sta = 0x03;
//		}
//	}
	
//	//修改地址后再尝试读
//  Status = VL53L0X_RdWord(&multi_vl53l0x_dev[5], VL53L0X_REG_IDENTIFICATION_MODEL_ID, &TmpData);
//  if(Status != VL53L0X_ERROR_NONE)
//  {
//    /*读取寄存器出错*/
//    sta = 0x02;
//    return;
//  }
//  if(TmpData == 0xEEAA)
//  {
//    /*设置设备新的I2C地址*/
//    Status = VL53L0X_SetDeviceAddress(&multi_vl53l0x_dev[5], 0x53);
//	}
	
}


void DBG_Test_Address(void)
{
	
	
}



void DBG_Reset_All(void)//add.cj  ps:Test ResetAll Device
{
	osDelay(5);
	
  for (int index = 0; index < 6; index++)
	{
		VL53L0X_ResetDevice(&multi_vl53l0x_dev[index]);
	}

		
}

uint8_t DBG_RegRef_C0[6] = {0};
uint8_t DBG_RegRef_C1[6] = {0};
uint8_t DBG_RegRef_C2[6] = {0};
uint16_t DBG_RegRef_51[6] = {0};
uint16_t DBG_RegRef_61[6] = {0};
void DBG_Read_ReferenceRegs(void)  //add.cj  ps:Test for read reference register
{
	osDelay(5);
	
  for (int index = 0; index < 6; index++)
  {
    VL53L0X_RdByte(&multi_vl53l0x_dev[index], 0xC0, &DBG_RegRef_C0[index]);
		osDelay(1);
		VL53L0X_RdByte(&multi_vl53l0x_dev[index], 0xC1, &DBG_RegRef_C1[index]);
		osDelay(1);
		VL53L0X_RdByte(&multi_vl53l0x_dev[index], 0xC2, &DBG_RegRef_C2[index]);
		osDelay(1);
		VL53L0X_RdWord(&multi_vl53l0x_dev[index], 0x51, &DBG_RegRef_51[index]);
		osDelay(1);
		VL53L0X_RdWord(&multi_vl53l0x_dev[index], 0x61, &DBG_RegRef_61[index]);
		osDelay(1);
  }
}

/**
 * @brief  VL53L0X主测试程序
 * @param  无
 * @retval 无
 */
void vl53l0x_start_measurement(void)
{
  /*vl53l0x初始化*/
  for (int index = 0; index < 6; index++)
  {
    measure_once(&multi_vl53l0x_dev[index], 0x54 + 2 * index);
    flow_control_paras.tranfer_complete = 1;
  }
	
}

/**
 * @brief  VL53L0X 测量模式配置
 * @param  dev:设备I2C参数结构体
 * @param  mode: 0:默认;1:高精度;2:长距离
 * @retval 状态信息
 */
VL53L0X_Error vl53l0x_set_mode(VL53L0X_Dev_t *dev, uint8_t mode)
{
  VL53L0X_Error status = VL53L0X_ERROR_NONE;
  uint8_t VhvSettings;
  uint8_t PhaseCal;
  uint32_t refSpadCount;
  uint8_t isApertureSpads;

  /*复位vl53l0x(频繁切换工作模式容易导致采集距离数据不准，需加上这一代码)*/
  status = vl53l0x_reset(dev);
  if(status != VL53L0X_ERROR_NONE)
  {
    goto error;
  }

  #if VL53L0X_DEBUG
  sprintf(VL53L0XMsg, "Reset OK\r\n");
  HAL_UART_Transmit(&huart4, (uint8_t *)VL53L0XMsg, strlen(VL53L0XMsg), 0xF);
  #endif
  
  status = VL53L0X_StaticInit(dev);
  if(status != VL53L0X_ERROR_NONE)
  {
    goto error;
  }
  
  #if VL53L0X_DEBUG
  sprintf(VL53L0XMsg, "Static Init OK\r\n");
  HAL_UART_Transmit(&huart4, (uint8_t *)VL53L0XMsg, strlen(VL53L0XMsg), 0xF);
  #endif

  /*已校准好了,写入校准值*/
  if (AjustOK != 0)
  {
    /*设定Spads校准值*/
    status = VL53L0X_SetReferenceSpads(dev, Vl53l0x_data.refSpadCount,
                                       Vl53l0x_data.isApertureSpads);
    if (status != VL53L0X_ERROR_NONE)
      goto error;
    hal_delay_ms(2);

    /*设定Ref校准值*/
    status = VL53L0X_SetRefCalibration(dev, Vl53l0x_data.VhvSettings,
                                       Vl53l0x_data.PhaseCal);
    if (status != VL53L0X_ERROR_NONE)
      goto error;
    hal_delay_ms(2);

    /*设定偏移校准值*/
    status = VL53L0X_SetOffsetCalibrationDataMicroMeter(dev,
                                                        Vl53l0x_data.OffsetMicroMeter);
    if (status != VL53L0X_ERROR_NONE)
      goto error;
    hal_delay_ms(2);

    /*设定串扰校准值*/
    status = VL53L0X_SetXTalkCompensationRateMegaCps(dev,
                                                     Vl53l0x_data.XTalkCompensationRateMegaCps);
    if (status != VL53L0X_ERROR_NONE)
      goto error;
    hal_delay_ms(2);
  }
  else
  {
    /*Ref参考校准*/
    status = VL53L0X_PerformRefCalibration(dev, &VhvSettings, &PhaseCal);
    if (status != VL53L0X_ERROR_NONE)
      goto error;
    hal_delay_ms(2);

    /*执行参考SPAD管理*/
    status = VL53L0X_PerformRefSpadManagement(dev, &refSpadCount,
                                              &isApertureSpads);
    if (status != VL53L0X_ERROR_NONE)
      goto error;
    hal_delay_ms(2);
  }

  /*使能单次测量模式*/
  status = VL53L0X_SetDeviceMode(dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
  if (status != VL53L0X_ERROR_NONE)
    goto error;
  hal_delay_ms(2);

  /*使能SIGMA范围检查*/
  status = VL53L0X_SetLimitCheckEnable(dev,
                                       VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
                                       1);
  if (status != VL53L0X_ERROR_NONE)
    goto error;
  hal_delay_ms(2);

  /*使能信号速率范围检查*/
  status = VL53L0X_SetLimitCheckEnable(dev,
                                       VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
                                       1);
  if (status != VL53L0X_ERROR_NONE)
    goto error;
  hal_delay_ms(2);

  /*设定SIGMA范围*/
  status = VL53L0X_SetLimitCheckValue(dev,
                                      VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
                                      Mode_data[mode].sigmaLimit);
  if (status != VL53L0X_ERROR_NONE)
    goto error;
  hal_delay_ms(2);

  /*设定信号速率范围范围*/
  status = VL53L0X_SetLimitCheckValue(dev,
                                      VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
                                      Mode_data[mode].signalLimit);
  if (status != VL53L0X_ERROR_NONE)
    goto error;
  hal_delay_ms(2);

  /*设定完整测距最长时间*/
  status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(dev,
                                                          Mode_data[mode].timingBudget);
  if (status != VL53L0X_ERROR_NONE)
    goto error;
  hal_delay_ms(2);

  /*设定VCSEL脉冲周期*/
  status = VL53L0X_SetVcselPulsePeriod(dev,
                                       VL53L0X_VCSEL_PERIOD_PRE_RANGE,
                                       Mode_data[mode].preRangeVcselPeriod);
  if (status != VL53L0X_ERROR_NONE)
    goto error;
  hal_delay_ms(2);

  /*设定VCSEL脉冲周期范围*/
  status = VL53L0X_SetVcselPulsePeriod(dev,
                                       VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
                                       Mode_data[mode].finalRangeVcselPeriod);

error: /*错误信息*/
  if (status != VL53L0X_ERROR_NONE)
  {
    print_pal_error(status);
    return status;
  }
  return status;
}

/**
 * @brief  VL53L0X 单次距离测量函数
 * @param  dev:设备I2C参数结构体
 * @param  pdata:保存测量数据结构体
 * @retval 状态信息
 */
VL53L0X_Error vl53l0x_start_single_test(VL53L0X_Dev_t *dev,
                                        VL53L0X_RangingMeasurementData_t *pdata,
                                        char *buf)
{
  VL53L0X_Error status = VL53L0X_ERROR_NONE;
  uint8_t RangeStatus;

  /*执行单次测距并获取测距测量数据*/
  status = VL53L0X_PerformSingleRangingMeasurement(dev, pdata);
  if (status != VL53L0X_ERROR_NONE)
	{
//		if(status == VL53L0X_ERROR_TIME_OUT)
//		{
//			status = VL53L0X_ERROR_TIME_OUT;
//		}
    return status;
	}

  /*获取当前测量状态*/
  RangeStatus = pdata->RangeStatus;
  memset(buf, 0x00, VL53L0X_MAX_STRING_LENGTH);
  /*根据测量状态读取状态字符串*/
  VL53L0X_GetRangeStatusString(RangeStatus, buf);
  /*保存最近一次测距测量数据*/
  dev->Distance_data = pdata->RangeMilliMeter;

  return status;
}

/**
 * @brief  启动普通测量
 * @param  dev：设备I2C参数结构体
 * @param  mode模式配置 0:默认;1:高精度;2:长距离
 * @retval 无
 */
void vl53l0x_general_start(VL53L0X_Dev_t *dev)
{
  /*测试模式字符串字符缓冲区*/
  static char buf[VL53L0X_MAX_STRING_LENGTH];
  /*工作状态*/
  VL53L0X_Error Status = VL53L0X_ERROR_NONE;

  /*执行一次测量	*/
  if (Status != vl53l0x_start_single_test(dev, &dev->vl53l0x_data, buf))
  {
    /*测量Not OK*/
    osDelay(1);
  }
}

/**
 * @brief  工作模式信息函数
 * @param  无
 * @retval 无
 */
void Show_GenTask_Message(void)
{
#if 0
  printf("   指令   ------      功能 \r\n");
  printf("    0     ------    默认测量模式 \r\n");
  printf("    1     ------    高精度测量模式 \r\n"); 
  printf("    2     ------    长距离测量模式 \r\n");
  printf("    3     ------    高速测量模式 \r\n"); 
  printf("***输入数字指令后，按回车键再发送*** \r\n");
#endif
}

/**
 * @brief  vl53l0x普通测量模式测试
 * @param  dev:设备I2C参数结构体
 * @retval 无
 */
void vl53l0x_general_test(VL53L0X_Dev_t *dev)
{
  vl53l0x_general_start(dev);
}

/*********************************************END OF FILE**********************/
